#ifndef CYPHY_SONAR
#define CYPHY_SONAR
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "std_msgs/Int32.h"
#include "std_msgs/Float32.h"
//#include "std_msgs/Float64.h"
#include <sstream>
#include <iostream>
#include <pthread.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/time.h>
#include <fcntl.h>
#include <termios.h>
#include <stdio.h>
#include <stdlib.h>
#include <vector>
#include <deque>
#include <numeric>
#include <functional>
#include <cyphy_kbd/KeyCode.h>

#define DEFAULT_BAUDRATE 115200
//#define DEFAULT_SERIALPORT "/dev/ttyACM1"
#define DEFAULT_SERIALPORT "/dev/ttyACM0"
#define ALPHA 0.8
//#define MOVING_AVG_WINDOW_SIZE 20
#define MOVING_AVG_WINDOW_SIZE 3


#define MIN_HEIGHT 0.2 //
//#define THRESHOLD_HEIGHT 5 // 
#define THRESHOLD_HEIGHT 9 // 
#define SAMP_TIME 0.05
#define HEIGHT_MAX 2.0 //0.6 //metre
#define BOUND 0.2
//#define BOUND 0.6


#define KEYCODE_SPACEBAR 0x20 //to start
#define KEYCODE_ESC 0x1B //to finish
// for rectangle path
#define KEYCODE_q 0x71 // 'q'
#define KEYCODE_w 0x77 // 'w'
#define KEYCODE_s 0x73 // 's'
#define KEYCODE_a 0x61 // 'a'

// for triangle path
#define KEYCODE_d 0x64 // 'd'
#define KEYCODE_f 0x66 // 'f'
#define KEYCODE_r 0x72 // 'r'
#define KEYCODE_e 0x65 // 'e'
#define KEYCODE_v 0x76 // 'v'
#define KEYCODE_t 0x74 // 't'
#define KEYCODE_9 0x39 // 't'





// for circle path
#define KEYCODE_c 0x63 // 'c'
enum {RECEIVE_START,ON_AIR,ON_LANDING};



//Global data
FILE *fpSerial = NULL;   //serial port file pointer
int fdSerial = NULL;   //serial port dev pointer
ros::Publisher ucResponseMsg;
ros::Subscriber ucCommandMsg;
FILE *fd=NULL;
ros::Time begin_time,current_time;
ros::Subscriber kbdSubscriber;

int window_size;
float avg;
int initialized;
int time_initialized=0;
std::vector<float> buf;
std::vector<float> sum;
std::deque<float> avg_;
std::deque<float> avg__;
std_msgs::Float32 height;
//std_msgs::Float64 height;

int quad_state=100;
int ucIndex;          //ucontroller index number
float preDistance=0;
float prev;
bool First=0;
bool start=0;

int serialInit(char * port, int baud);
void ucCommandCallback(const std_msgs::String::ConstPtr& msg);
void kbdCallback (const cyphy_kbd::KeyCode& msg);
void *rcvThread(void *arg);
float LFP(float distance);


#endif
